#include "projector.h"
#include "sharedStructure.hpp"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <iostream>

#include <deque>

#include "Eigen/SVD"
#include "Eigen/Dense"


#include "AR/ar.h"


#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

void projector::decomposeP(float src_P[3][4], double rot[3][3], double cam_dir[3],double trans[3], double cparam[3][4]){
	 Eigen::MatrixXd core(3,3);
	 Eigen::MatrixXd KP(3,1);
	 for( int row = 0; row<3; ++row){
		 for(int col = 0; col<3; ++col){
			core(row,col)=src_P[row][col];
		 }
		KP(row,0) = src_P[row][3];
	 }

	/*follows are vgg_RQ(core)*/
	core.transposeInPlace();
	core.reverseInPlace();

	Eigen::MatrixXd QR=core.householderQr().matrixQR();
	Eigen::MatrixXd K(3,3); 
	K.setZero();
	for( int i = 0; i<3; ++i){
		for(int j = i; j<3 ; ++j){
			K(i,j)=QR(i,j);
		}
	}
	
	K.transposeInPlace();
	K.reverseInPlace();

	core.reverseInPlace();
	core.transposeInPlace();
	Eigen::MatrixXd R = K.inverse()*core;
	
	if(R.determinant()<0){
		K.col(0)*=-1.0;
		R.row(0)*=-1.0;
	}
	/*end of vgg_RQ(core)*/


	/*debug session*/
	//std::cout<<"KR:\n"<<core<<std::endl;
	//std::cout<<"KT:\n"<<KP<<std::endl;
	//std::cout<<"K:\n"<<K<<std::endl;
	//std::cout<<"R:\n"<<R<<std::endl;
	//std::cout<<"K*R:\n"<<K*R<<std::endl;


	double ratio = K(2,2);
	K = K/ratio;
	if( K(0,0)<0){
		//Eigen::MatrixXd D = Eigen::MatrixXd::Zero(3);
		Eigen::MatrixXd D(3,3);
		D.setZero();
		D(0,0) = -1; D(1,1)=-1; D(2,2)=1;
		K = K * D;
		R = D * R;
	}
	Eigen::Vector3d tVec= K.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(KP/ratio);

	for(int row = 0; row<3; ++row){
		for(int col =0; col<3; ++col){
			rot[row][col]=R(row,col);
			cparam[row][col] = K(row,col);
		}
		trans[row]=tVec(row);
		cparam[row][3]=0.0;
	}

	if( cam_dir !=NULL){
		for(int row = 0; row<3; ++row)
				cam_dir[row]=R(2,row);
	}

	/*debug session*/
	//Eigen::MatrixXd Pmat(3,4);
	//Pmat.block<3,3>(0,0)=core;
	//Pmat.block<3,1>(0,3)=KP;
	//Pmat = Pmat/Pmat(2,3);

	//std::cout<<"unified src_P"<<Pmat<<std::endl;

	//std::cout<<"K:\n"<<K<<std::endl;
	//std::cout<<"R:\n"<<R<<std::endl;
	//std::cout<<"T:\n"<<tVec<<std::endl;
	//Pmat.block<3,3>(0,0)=K*R;
	//Pmat.block<3,1>(0,3)=K*tVec;
	//Pmat = Pmat/Pmat(2,3);

	//std::cout<<"unified est_P"<<Pmat<<std::endl;

 }


 void projector::testPdecomposition(){
	 float Pmat[3][4]={  {-0.072337251259602 ,  0.082678887893760  , 0.000000000001258  ,79.908764133397867},
	 {   0.019520107273205   ,0.017078494164098   ,0.106750957243943 ,-60.118061164998096},
	 {  -0.000053011953824  ,-0.000046381115186   ,0.000017113838379  ,-0.619676912942076}};

	 double rot[3][3];
	 double trans[3];
	 double cam_dir[3];
	 double cparam[3][4];
	 projector::decomposeP(Pmat,rot,cam_dir,trans,cparam);

 }


 void projector::testPespEstimation(){
	 float Pmat[3][4]={  {-0.072337251259602 ,  0.082678887893760  , 0.000000000001258  ,79.908764133397867},
	 {   0.019520107273205   ,0.017078494164098   ,0.106750957243943 ,-60.118061164998096},
	 {  -0.000053011953824  ,-0.000046381115186   ,0.000017113838379  ,-0.619676912942076}};

	 std::deque<pt_pair_s> pts;
	// read data from file
	const char* file_name = "D:/Projects/AoT/testCode/detection/frame_pts_v2.csv";
	FILE *p_file = fopen(file_name,"r");
	if( p_file == NULL){		
		const char* file_name_2 = "/home/wzhu/Documents/CarAoT/testCode/detection/frame_pts_v2.csv";
		p_file = fopen(file_name_2,"r");
		
		if(p_file==NULL)		
			printf("Cannot open file :  NOR: %s\n",file_name, file_name_2);
		return;
	}
		// here we know that there are 28 points in the frame_pts_v2.csv
		float X[28][3];
		float x[28][2];
		for(int i = 0; i<28; ++i){
			fscanf(p_file,"%f,%f,%f",&X[i][0],&X[i][1],&X[i][2]);
			//X[i][3]=1;
		}
		fclose(p_file);
		// project the data points accordint to P
		for(int i = 0; i<16; ++i){
			x[i][0] = Pmat[0][0]*X[i][0]+Pmat[0][1]*X[i][1]+Pmat[0][2]*X[i][2]+Pmat[0][3];
			x[i][1] = Pmat[1][0]*X[i][0]+Pmat[1][1]*X[i][1]+Pmat[1][2]*X[i][2]+Pmat[1][3];
			x[i][2] = Pmat[2][0]*X[i][0]+Pmat[2][1]*X[i][1]+Pmat[2][2]*X[i][2]+Pmat[2][3];

			x[i][0] /=x[i][2];	x[i][1] /=x[i][2];
		pt_pair_s pt;
		pt[0]=X[i][0];pt[1]=X[i][1];pt[2]=X[i][2];
		pt[3]=x[i][0];pt[4]=x[i][1];
		pts.push_back(pt);
		}
		float cam_dir[3];
		float error;
		projector::estimatePerspectiveTransform(pts,Pmat,cam_dir,error);
		float ratio = -0.619676912942076/Pmat[2][3];
		for(int row = 0; row<3 ; ++row){
			for(int col = 0; col<4; ++col){
				printf("%f\t",Pmat[row][col]*ratio);
			}
			printf("\n");
		}

 }

 bool projector::estimateOrthognalTransform(const std::deque<pt_pair_s> &ptps, float dest_P[][4],float dir[3], float &error){
	int n_pts = ptps.size();
	Eigen::MatrixXf A8(2*n_pts,8);
	Eigen::MatrixXf b(2*n_pts,1);

	for( int i_pt =0; i_pt<n_pts; ++i_pt){
		A8.row(2*i_pt)<<ptps[i_pt][0],ptps[i_pt][1],ptps[i_pt][2],1.0f,0.f,0.f,0.f,0.f;
		A8.row(2*i_pt+1)<<0.f,0.f,0.f,0.f,ptps[i_pt][0],ptps[i_pt][1],ptps[i_pt][2],1.0f;
		b(2*i_pt,0)=ptps[i_pt][3];
		b(2*i_pt+1,0)=ptps[i_pt][4];
	}
	
	/*trival method*/
	//Eigen::MatrixXf P8= A8.fullPivHouseholderQr().solve(b); 
	//Eigen::MatrixXf P8 = A8.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
	
	/*pseduo inverse method*/
	
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(A8,Eigen::ComputeThinU | Eigen::ComputeThinV);

	double epsilon =std::numeric_limits<float>::epsilon();
	double tolerance = epsilon * std::max(A8.cols(), A8.rows()) * svd.singularValues().cwiseAbs().maxCoeff();
	Eigen::VectorXf D = svd.singularValues();
	int m = D.rows();
	for(int i = 0; i<m; ++i){
		if (abs(D(i))<tolerance) D(i)=0.0f;
		else D(i)=1.0/D(i);
	}
	Eigen::MatrixXf inv_A8=svd.matrixV() * (D.asDiagonal()) * svd.matrixU().transpose();
	Eigen::MatrixXf P8=inv_A8*b;
	
	//std::cout<<"A8"<<std::endl<<A8<<std::endl;
	//std::cout<<"b"<<std::endl<<b<<std::endl;
	//std::cout<<"P8"<<std::endl<<P8<<std::endl;


	P8.resize(4,2);
	P8.transposeInPlace();


	//std::cout<<"P8 after resize"<<std::endl<<P8<<std::endl;


	Eigen::MatrixXf P(3,4);
	P.block<2,4>(0,0)=P8;
	P.row(2)<<0.0f,0.0f,0.0f,1.0f;
	error = 0;
	for(int i_pt = 0; i_pt<n_pts; ++i_pt){
		Eigen::Vector4f pt_3D;
		const pt_pair_s &ref_pair = ptps[i_pt];
		pt_3D<<ref_pair[0],ref_pair[1],ref_pair[2],1.0f;
		Eigen::Vector3f res_2D = P*pt_3D;
		float dx  = res_2D(0)/res_2D(2)-ref_pair[3];
		float dy = res_2D(1)/res_2D(2)-ref_pair[4];
		float e_pt = dx*dx+dy*dy;
		error +=sqrt(e_pt);
	}
	error/=n_pts;

	for( int row = 0; row<3; ++row){
		for( int col = 0; col<4; ++col){
			dest_P[row][col]=P(row,col);
		}
	}


	

	/*now cam direction*/
	Eigen::Vector3f cam_dir_vec;
	Eigen::MatrixXf M = P8.block<2,3>(0,0);

	Eigen::JacobiSVD<Eigen::MatrixXf> svdM(M,Eigen::ComputeFullV);
	//svdM.computeV();
	Eigen::MatrixXf V = svdM.matrixV();
	Eigen::MatrixXf cam_dir =V.col(2);
	if(cam_dir(2,0)>0){/*here we assume the cam_dir is always looking downward*/
		cam_dir = -cam_dir;
	}
	for( int i_el = 0; i_el<3; ++i_el){
		dir[i_el]=cam_dir(i_el);
	}
	return true;
}


bool projector::estimatePByPlaneInfo(const std::deque<pt_pair_s> &pts,int img_width, int img_height, float dest_P[][4], float cam_dir[], float &error){
#ifdef USE_OPEN_CV
	return this->estimatePerspectiveTransform(pts,dest_P,cam_dir,error);
	
#else
	int npt	=pts.size();
	
	std::deque<pt_pair_s> Dpairs; 
	
	for(int i =0; i<npt; ++i){
		pt_pair_s tmp;
		this->worldPoint2Plane(pts[i][0],pts[i][1],pts[i][2],tmp[0],tmp[1],tmp[2]);
		if( abs(tmp[2])>1e-4){
			printf("warning, point not ON plane!\n");
		}
		tmp[3]=pts[i][3];
		tmp[4]=pts[i][4];
		Dpairs.push_back(tmp);
	}

	/*estimate H*/
	float H[3][3]; 
	double P[3][4];
	float err;
	this->estimateH(Dpairs,H,err);
	

	/*using H, project the four vertex points, and fit the line function*/
	float dim = this->w<this->h? h: w;
	dim = dim/s_wt;
	dim = dim/s_tp;

	ARMarkerInfo marker;
	marker.area =  w*h;
	//center
	float cx,cy;
	this->projectH(H,0,0,cx,cy);
	marker.pos[0]=cx;
	marker.pos[1]=cy;
	/*four vertex clock wise*/
	float model_vertex[][2]={{0,dim},{dim,dim},{dim,0},{0,0}};
	float px,py;
	for(int i = 0; i<4; ++i){
		px = model_vertex[i][0]*s_wt*s_tp;
		py = model_vertex[i][1]*s_wt*s_tp;
		this->projectH(H,px,py,cx,cy);
		marker.vertex[i][0]=cx;
		marker.vertex[i][1]=cy;
	}

	//this->estimateOrthognalTransform(pts,dest_P,cam_dir,error);
	//float Pp[12];
	//for(int row =0; row<3; ++row){
	//	for(int col= 0; col<4; ++col){
	//		Pp[col*3+row]=dest_P[row][col];
	//	}
	//}

	//this->updateCamera(Pp,cam_dir,1.0f,1.0f);

	//float dim = this->w>this->h? h: w;
	//dim = dim/s_wt;
	//dim = dim/s_tp;

	//ARMarkerInfo marker;
	//marker.area =  w*h;
	//float cx,cy;
	//this->planePoint2Image(0,0,cx,cy);
	//marker.pos[0]=cx;marker.pos[1] = cy;
	//float model_vertex[][2]={{0,dim},{dim,dim},{dim,0},{0,0}};
	////	float img_vertex[][2]={{47,-23},{62,13},{18,12},{3,-30}};
	////float img_vertex[][2]={{247,127},{262,163},{218,162},{203,120}};
	//float px,py;
	//for( int i = 0; i<4; ++i){
	//	px = model_vertex[i][0]*s_wt*s_tp;
	//	py = model_vertex[i][1]*s_wt*s_tp;
	//	this->planePoint2Image(px,py,cx,cy);
	//	// // resize the piont coordinate, to make it compatible with resized plane coordinate in following estimation
	//	marker.vertex[i][0]=cx;
	//	marker.vertex[i][1]=cy;
	//}




	/*four lines*/
	for( int i= 0; i<4; ++i){
		this->fit2DLine(marker.vertex[i][0],marker.vertex[i][1],marker.vertex[(i+1)%4][0],marker.vertex[(i+1)%4][1],marker.line[i]);
	}
	
	marker.dir = 0;
	/*get the projection matrix from AR Tool kit, note that called code is not the original code, because T is revised.*/
	this->initARToolkit(img_width,img_height);

	double center[2]={0,0};
	double PDouble[3][4];
	double ar_error = arGetTransMat(&marker,center,dim,PDouble);


	/*add transofmormation from wrold to plane, with origin at plane center, u top-down and v left-right*/
	Eigen::MatrixXf Pmat(3,4);
	for(int i = 0; i<3; ++i){
		for( int j = 0; j<4; ++j){
			Pmat(i,j)=(float)PDouble[i][j];
		}
	}
	Eigen::MatrixXf Rwpp=this->Rwp;
	//std::cout<<"Rwpp\n"<<Rwpp<<std::endl;
	//std::cout<<"Pmat\n"<<Pmat<<std::endl;

	double coef=1/s_wt;
	coef /=s_tp;
	Rwpp.block<3,4>(0,0)=coef*Rwpp.block<3,4>(0,0);
	//std::cout<<"Rwpp\n"<<Rwpp<<std::endl;
	Pmat = Pmat*Rwpp;
	//std::cout<<"Pmat\n"<<Pmat<<std::endl;

	/*test the error using the vertex of the plane, to see if consistant after applying Rwpp*/
	error=0;
	for(int i_pt=0; i_pt<4; ++i_pt){
		px = model_vertex[i_pt][0]*s_wt*s_tp;
		py = model_vertex[i_pt][1]*s_wt*s_tp;
		float X,Y,Z;
		this->planePoint2World(px,py,X,Y,Z);
		Eigen::Vector4f src_pt;
		src_pt<<X,Y,Z,1.0f;
		Eigen::Vector3f res_2D = Pmat*src_pt;
		float dx  = res_2D(0)/res_2D(2)-marker.vertex[i_pt][0];
		float dy = res_2D(1)/res_2D(2)-marker.vertex[i_pt][1];
		float e_pt = dx*dx+dy*dy;
		error += (e_pt);
	}
	error/=4;



	error = 0;
	for(int i_pt = 0; i_pt<npt; ++i_pt){
		Eigen::Vector4f pt_3D;
		const pt_pair_s &ref_pair = pts[i_pt];
		pt_3D<<ref_pair[0],ref_pair[1],ref_pair[2],1.0f;
		Eigen::Vector3f res_2D = Pmat*pt_3D;
		float dx  = res_2D(0)/res_2D(2)-ref_pair[3];
		float dy = res_2D(1)/res_2D(2)-ref_pair[4];
		float e_pt = dx*dx+dy*dy;
		error += (e_pt);
	}
	error/=npt;


	for(int i = 0; i<3; ++i){
		for( int j = 0; j<4; ++j){
			dest_P[i][j]=Pmat(i,j);
		}
	}



	Eigen::Vector3f dir;
	cam_direction(Pmat,dir);
	cam_dir[0]=dir(0);
	cam_dir[1]=dir(1);
	cam_dir[2]=dir(2);
	return true;
#endif
	}

bool projector::projectH(float H[][3], float xr, float yr, float &xl, float &yl){

	float wl;
	
	xl = H[0][0]*xr + H[0][1]*yr + H[0][2];
	yl = H[1][0]*xr + H[1][1]*yr + H[1][2];
	wl = H[2][0]*xr + H[2][1]*yr + H[2][2];

	xl /=wl;
	yl /=wl;
	return true;
}




void projector::initARToolkit( int width, int height )
{
printf("ERROR: function initARToolkit is disabled.\n");
#if 0
    ARParam  wparam;
	ARParam cparam;
    /* set the initial camera parameters */
	const char  *cparam_name    = "D:/Projects/AoT/testCode/detection/camera_para.dat";
    if( arParamLoad(cparam_name, 1, &wparam) < 0 ) {
        printf("Camera parameter load error !!\n");
        exit(0);
    }
    arParamChangeSize( &wparam, width, height, &cparam );
    arInitCparam( &cparam );
    printf("*** Camera Parameter ***\n");
    arParamDisp( &cparam );
#endif
}

bool projector::estimateH(const std::deque<pt_pair_s> &pts, float dest_H[][3], float &error){
	int npt = pts.size();

#ifdef USE_OPEN_CV
	
#else
	Eigen::MatrixXf A(2*npt,9);
	/*w_i' and w_i in Eqn 4.1 should be 1*/
	for( int i =0; i<npt; ++i){
		A.row(i*2)<<0.f,0.f,0.f,-pts[i][0],-pts[i][1],-1.0f,pts[i][0]*pts[i][4],pts[i][1]*pts[i][4],pts[i][4];
		A.row(i*2+1)<<pts[i][0],pts[i][1],1,0.f,0.f,0.f,-pts[i][3]*pts[i][0],-pts[i][3]*pts[i][1],-pts[i][3];
	}
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(A,Eigen::ComputeThinU | Eigen::ComputeThinV);

	Eigen::MatrixXf V = svd.matrixV();
	int m = V.cols();
	Eigen::MatrixXf resH = V.col(m-1);
	
	resH.resize(3,3);
	resH.transposeInPlace();

	error  =0;
	for(int i = 0; i<npt; ++i){
		Eigen::Vector3f src,dst;
		src<<pts[i][0],pts[i][1],1.0f;
		dst = resH*src;
		float dx  = dst(0)/dst(2)-pts[i][3];
		float dy = dst(1)/dst(2)-pts[i][4];
		float e_pt = dx*dx+dy*dy;
		error += sqrt(e_pt);
	}
	error /=npt;

	for(int row = 0; row<3; ++row){
		for(int col = 0; col<3; ++col){
			dest_H[row][col] = resH(row,col);
		}
	}

#endif
	return true;

}

bool projector::worldPoint2Plane(float X, float Y, float Z, float &u, float &v, float &w){
	Eigen::Vector4f W;
	W<<X,Y,Z,1.0f;

	Eigen::Vector4f Wp;
	Wp = this->Rwp*W;
	u = Wp(0);
	v = Wp(1);
	w = Wp(2);


	//float Xt, Yt, Zt;
	//worldPoint2Tan(X,Y,Z,Xt,Yt,Zt);
	//tanPoint2Plane(Xt,Yt,Zt,u,v);

	return true;
}



bool projector::worldPoint2Tan(float X, float Y, float Z, float &Xt,float &Yt, float &Zt){
	float Xtt = X+T[0]; 
	float Ytt = Y + T[1];
	float Ztt = Z + T[2];

	Xt = R[0][0]*Xtt + R[0][1]*Ytt + R[0][1]*Ztt;
	Yt = R[1][0]*Xtt + R[1][1]*Ytt + R[1][1]*Ztt;
	Zt = R[2][0]*Xtt + R[2][1]*Ytt + R[2][1]*Ztt;

	Xt*=s_wt;
	Yt*=s_wt;
	Zt*=s_wt;
	return true;
}

bool projector::tanPoint2Plane(float Xt, float Yt, float Zt, float &a, float &b){
	float Xtt = Xt -plane_origin[0];
	float Ytt = Yt - plane_origin[1];
	float Ztt = Zt - plane_origin[2];

	a = u[0]*Xtt + u[1]*Ytt + u[2]*Ztt;
	b = v[0]*Xtt + v[1]*Ytt + v[2]*Ztt;

	a*=s_tp;
	b*=s_tp;
	return true;
}


void projector::computePwp(){
	float w[3];

	w[0]=u[1]*v[2]-u[2]*v[1];
	w[1]=u[0]*v[2]-u[2]*v[0];
	w[2]=u[0]*v[1]-u[1]*v[0];
	w[1]=-w[1];

	Eigen::MatrixXf Rmat(3,3);
	Eigen::MatrixXf Ruv(3,3);
	Eigen::MatrixXf RuvT(3,3);
	Eigen::Vector3f Tvec,Op;

	/**/
	for( int i = 0; i<3; ++i){
		for( int j =0; j<3; ++j){
			Rmat(i,j)=R[i][j];
		}
		Tvec(i) = T[i];
		Op(i) = plane_origin[i];
	}
	Ruv.col(0)<<u[0],u[1],u[2];
	Ruv.col(1)<<v[0],v[1],v[2];
	Ruv.col(2)<<w[0],w[1],w[2];
	RuvT = Ruv.transpose();
	/*begin*/
	Rwp=Eigen::MatrixXf(4,4);
	
	
	Rwp.block<3,3>(0,0) = s_tp*RuvT*Rmat*s_wt;
	Rwp.block<3,1>(0,3) = s_tp*s_wt*RuvT*Rmat*Tvec-s_tp*RuvT*Op;
	Rwp.row(3)<<0.0f,0.0f,0.0f,1.0f;
	

	/*printf("R:\n");
	std::cout<< Rmat<<std::endl;
	printf("Ruv\n");
	std::cout<< Ruv<<std::endl;
	printf("RuvT\n");
	std::cout<< RuvT<<std::endl;
	printf("R33\n");
	std::cout<<s_tp*RuvT*Rmat*s_wt<<std::endl;
	printf("R41\n");
	std::cout<<s_tp*s_wt*RuvT*Rmat*Tvec-s_tp*RuvT*Op<<std::endl;
	printf("Rwp\n");
	std::cout<< Rwp<<std::endl;*/
}

void projector::fit2DLine(float x1, float y1, float x2, float y2, double n[3]){

	Eigen::MatrixXf A(2,3);
	A.row(0)<<x1,y1,1.0f;
	A.row(1)<<x2,y2,1.0f;
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(A,Eigen::ComputeThinU | Eigen::ComputeFullV);

	Eigen::MatrixXf sol = svd.matrixV().col(2);
	float norm = sol(0)*sol(0)+sol(1)*sol(1);
	norm = sqrt(norm);
	n[0]=sol(0)/norm;
	n[1]=sol(1)/norm;
	n[2]=sol(2)/norm;

	//std::cout<<"A"<<std::endl<<A<<std::endl;
	//std::cout<<"nt: "<<n[0]<<","<<n[1]<<","<<n[2]<<std::endl;
}
